//~ #include "pid.h"

//~ void L298NA(float V,float angle)
//~ {
	//~ softPwmWrite(PWM1,V+angle);
    //~ softPwmWrite(PWM3,V-angle);
//~ }

//~ void L298NB(float V,float angle)
//~ {
	//~ softPwmWrite(PWM4,V+angle);
    //~ softPwmWrite(PWM2,V-angle);
//~ }

//~ void panduan()
//~ {
	//~ if(FL<0)panduan_FL=-FL;
	//~ else
	//~ {
		//~ panduan_FL=FL;
	//~ }
	//~ if(FR<0)panduan_FR=-FR;
	//~ else
	//~ {
		//~ panduan_FR=FR;
	//~ }
//~ }

//~ void PID()
//~ {
	//~ while (1)
	//~ {	
	//~ float PID_OUT;
	//~ float kp=2;
	//~ float ki=0;
	//~ float kd=1.5;
	//~ float this_error=0,last_error,kp_error,ki_error,kd_error,angle1,angle2;
	//~ float sum;
	 //~ for(i=1;i>0;i++)
//~ { 
		//~ kp_error=now_angle-target_angle;
		//~ ki_error+=now_angle-target_angle;
		//~ if(ki_error>=5)ki_error=5;
		//~ if(i%2==1)
		//~ {last_error=now_angle-target_angle;}
		//~ if(i%2==0)
		//~ {this_error=now_angle-target_angle;}
		//~ if(i%2==0)
		//~ {kd_error=this_error-last_error;}
		//~ if(i%2==1)
		//~ {kd_error=last_error-this_error;}
		//~ PID_OUT=kp*kp_error+ki*ki_error+kd*kd_error;
		//~ if(PID_OUT>15)
		//~ {PID_OUT=15;}
		//~ if(PID_OUT<-15)
		//~ {PID_OUT=-15;}
		
		//~ panduan();
		//~ if(panduan_FL>=panduan_FR)
		//~ {
		//~ L298NA(10,PID_OUT);
	    //~ }
	    //~ else
	    //~ {
		//~ L298NB(10,PID_OUT);
		//~ }
		//~ delay(20);
	//~ }
//~ }
//~ }
